﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Microsoft.DirectX;
using Microsoft.DirectX.Direct3D;

namespace Final_Project
{
    public class CIcpAlg
    {
        private int m_nRows;
        private int m_nColumns;
        private double m_dVEpsilon;
        private double m_dNEpsilon;
        private Matrix m_Transform;
        private Microsoft.DirectX.Vector4[] vList = new Microsoft.DirectX.Vector4[6];
        private Microsoft.DirectX.Vector4[] cList = new Microsoft.DirectX.Vector4[6];

        public CIcpAlg()
        {
            m_nRows = Convert.ToInt32(CXmlConfigReader.Instance().ReadValueFromElement("DepthMapResolutionRows"));
            m_nColumns = Convert.ToInt32(CXmlConfigReader.Instance().ReadValueFromElement("DepthMapResolutionColumns"));
            m_dVEpsilon = Convert.ToDouble(CXmlConfigReader.Instance().ReadValueFromElement("VertexDistanceEpsilon"));
            m_dNEpsilon = Convert.ToDouble(CXmlConfigReader.Instance().ReadValueFromElement("NormalDistanceEpsilon"));
            m_Transform = new Matrix();
        }

        public Matrix RunICP(ref Matrix PrevTMatrix, ref Microsoft.DirectX.Vector4[,] ViMatrix, ref Microsoft.DirectX.Vector4[,] PrevViMatrix, ref Microsoft.DirectX.Vector4[,] NiMatrix, ref Microsoft.DirectX.Vector4[,] PrevNiMatrix, ref Matrix PerspectiveMatrix)
        {
            Microsoft.DirectX.Vector4[,] ProjPrevViMatrix = new Microsoft.DirectX.Vector4[m_nRows, m_nColumns];
            Microsoft.DirectX.Vector4 A = new Microsoft.DirectX.Vector4();
            Microsoft.DirectX.Vector4 B = new Microsoft.DirectX.Vector4();
            Microsoft.DirectX.Vector4 C = new Microsoft.DirectX.Vector4();
            Microsoft.DirectX.Vector4 N;
            int nNum = 0;
            Boolean bFoundPoint = false;
            float fNormalsProduct;
            double x,y;

            PrevTMatrix.Invert();
            for (int i = 0; i < m_nColumns; i++)
            {
                for (int j = 0; j < m_nRows; j++)
                {
                    if (ViMatrix[i, j].Z > 0 && (false == VerifyVertexIsUnique(ref ViMatrix[i, j], ref cList)))
                    {
                        A.X = ViMatrix[i, j].X;
                        A.Y = ViMatrix[i, j].Y;
                        A.Z = ViMatrix[i, j].Z;
                        A.W = ViMatrix[i, j].W;
                        A.X = PrevTMatrix.M11 * A.X + PrevTMatrix.M12 * A.Y + PrevTMatrix.M13 * A.Z + PrevTMatrix.M14 * A.W;
                        A.Y = PrevTMatrix.M21 * A.X + PrevTMatrix.M22 * A.Y + PrevTMatrix.M23 * A.Z + PrevTMatrix.M24 * A.W;
                        A.Z = PrevTMatrix.M31 * A.X + PrevTMatrix.M32 * A.Y + PrevTMatrix.M33 * A.Z + PrevTMatrix.M34 * A.W;
                        // project A
                        B.X = (PerspectiveMatrix.M11 * A.X / A.Z + PerspectiveMatrix.M13);
                        B.Y = (PerspectiveMatrix.M22 * A.Y / A.Z + PerspectiveMatrix.M23);
                        if (B.X > 0 && B.X < m_nColumns && B.Y > 0 && B.Y < m_nRows)
                        {
                            x = Math.Ceiling(B.X);
                            y = Math.Ceiling(B.Y);
                            x--;
                            y--;
                            if (PrevViMatrix[(int)x, (int)y].Z > 0)
                            {
                                C.X = ViMatrix[i, j].X - PrevViMatrix[(int)x, (int)y].X;
                                C.Y = ViMatrix[i, j].Y - PrevViMatrix[(int)x, (int)y].Y;
                                C.Z = ViMatrix[i, j].Z - PrevViMatrix[(int)x, (int)y].Z;
                                C.W = 0;
                                N = PrevNiMatrix[(int)x, (int)y];
                                fNormalsProduct = Math.Abs(NiMatrix[i, j].X * N.X + NiMatrix[i, j].Y * N.Y + NiMatrix[i, j].Z * N.Z);
                                if (C.Length() < m_dVEpsilon/1000 && fNormalsProduct < m_dNEpsilon && nNum < 6)
                                {
                                    if (false == VerifyVertexIsUnique(ref PrevViMatrix[(int)x, (int)y], ref vList))
                                    {
                                        bFoundPoint = true;
                                        cList[nNum] = ViMatrix[i, j];
                                        vList[nNum] = PrevViMatrix[(int)x, (int)y];
                                        nNum++;
                                    }
                                }
                            }
                        }
                    }
                    if (nNum > 6)
                    {
                        break;
                    }
                }
            }
            FindTransformMatrix();
            return m_Transform;
            /*
            PrevTMatrix.Invert();

            for (int i = 0; i < m_nColumns; i++)
            {
                for (int j = 0; j < m_nRows; j++)
                {
                    // if depth value is valid
                    if (PrevViMatrix[i, j].Z > 0 && (false == VerifyVertexIsUnique(ref PrevViMatrix[i, j], ref vList)))
                    {
                        // find corresponding point
                        for (int k = 0; k < m_nColumns; k++)
                        {
                            for (int v = 0; v < m_nRows; v++)
                            {
                                if (ViMatrix[k, v].Z > 0)
                                {
                                    A.X = PrevTMatrix.M11 * ViMatrix[k, v].X + PrevTMatrix.M12 * ViMatrix[k, v].Y + PrevTMatrix.M13 * ViMatrix[k, v].Z;
                                    A.Y = PrevTMatrix.M21 * ViMatrix[k, v].X + PrevTMatrix.M22 * ViMatrix[k, v].Y + PrevTMatrix.M23 * ViMatrix[k, v].Z;
                                    A.Z = PrevTMatrix.M31 * ViMatrix[k, v].X + PrevTMatrix.M32 * ViMatrix[k, v].Y + PrevTMatrix.M33 * ViMatrix[k, v].Z;
                                    A.W = 0;
                                    N = NiMatrix[k, v];
                                    A.Subtract(PrevViMatrix[i, j]);
                                    fNormalsProduct = Math.Abs(PrevNiMatrix[i, j].X * N.X + PrevNiMatrix[i, j].Y * N.Y + PrevNiMatrix[i, j].Z * N.Z);
                                    if (A.Length() < m_dVEpsilon && fNormalsProduct < m_dNEpsilon && nNum < 6)
                                    {
                                        if (false == VerifyVertexIsUnique(ref A, ref cList))
                                        {
                                            bFoundPoint = true;
                                            cList[nNum] = ViMatrix[k, v];
                                            vList[nNum] = PrevViMatrix[i, j];
                                            nNum++;
                                        }
                                    }
                                }
                                if (bFoundPoint == true)
                                {
                                    break;
                                }
                            } // for (int v = 0; v < m_nColumns; v++)
                            if (bFoundPoint == true)
                            {
                                break;
                            }
                        } // for (int k = 0; k < m_nRows; k++)
                         
                    } // if(PrevViMatrix[i,j].Z > 0 && nNum < 6)
                    if (bFoundPoint == true)
                    {
                        bFoundPoint = false;
                        break;
                    }
                } // for (int j = 0; j < m_nColumns; j++)
                if (nNum >= 6)
                {
                    break;
                }
            } // for (int i = 0; i < m_nRows; i++)
            */

        }

        public bool VerifyVertexIsUnique(ref Microsoft.DirectX.Vector4 A, ref Microsoft.DirectX.Vector4[] List)
        {
            for (int i = 0; i < List.Length; i++)
            {
                if(List[i].X == A.X)
                    return true;
                if(List[i].Y == A.Y)
                    return true;
                if (List[i].Z == A.Z)
                    return true;
            }
            return false;
        }

        public void FindTransformMatrix()
        {

            double pi1, pi2, pi3, mu1, mu2, mu3;
            Microsoft.DirectX.Vector4 A = vList[0];
            Microsoft.DirectX.Vector4 B = cList[0];
            Microsoft.DirectX.Vector4 C = vList[1];
            Microsoft.DirectX.Vector4 D = cList[1];
            Microsoft.DirectX.Vector4 F = vList[2];
            Microsoft.DirectX.Vector4 G = cList[2];
            Microsoft.DirectX.Vector4 H = vList[3];
            Microsoft.DirectX.Vector4 J = cList[3];
            Microsoft.DirectX.Vector4 V = vList[4];
            Microsoft.DirectX.Vector4 L = cList[4];
            Microsoft.DirectX.Vector4 Z = vList[5];
            Microsoft.DirectX.Vector4 K = cList[5];

            // compute first row of T matrix
            pi1 = ((V.Y - Z.Y) - ((A.Y - C.Y) / (A.X - C.X)) * (V.X - Z.X));
            pi2 = ((V.Z - Z.Z) - ((A.Z - C.Z) / (A.X - C.X)) * (V.X - Z.X));
            pi3 = (((B.X - D.X) / (A.X - C.X)) * (V.X - Z.X) - (L.X - K.X));
            mu1 = (F.Y - H.Y) / (F.X - H.X) - (A.Y - C.Y) / (A.X - C.X);
            mu2 = (F.Z - H.Z) / (F.X - H.X) - (A.Z - C.Z) / (A.X - C.X);
            mu3 = (B.X - D.X) / (A.X - C.X) - (G.X - J.X) / (F.X - H.X);
            m_Transform.M13 = (float)(((mu3 / mu1) * pi1 - pi3) / (pi2 - pi1 * (mu2 / mu1)));
            m_Transform.M12 = (float)(-1 * ((mu2 / mu1) * m_Transform.M13 + (mu3 / mu1)));
            m_Transform.M11 = ((B.X - D.X) / (A.X - C.X) - (m_Transform.M12) * (A.Y - C.Y) / (A.X - C.X)) - (m_Transform.M13 * (A.Z - C.Z) / (A.X - C.X));
            m_Transform.M14 = B.X - (A.X * m_Transform.M11) - (A.Y * m_Transform.M12) - (A.Z * m_Transform.M13);

            // compute second row of T matrix
            pi1 = ((V.Y - Z.Y) - ((A.Y - C.Y) / (A.X - C.X)) * (V.X - Z.X));
            pi2 = ((V.Z - Z.Z) - ((A.Z - C.Z) / (A.X - C.X)) * (V.X - Z.X));
            pi3 = (((B.Y - D.Y) / (A.X - C.X)) * (V.X - Z.X) - (L.Y - K.Y));
            mu1 = (F.Y - H.Y) / (F.X - H.X) - (A.Y - C.Y) / (A.X - C.X);
            mu2 = (F.Z - H.Z) / (F.X - H.X) - (A.Z - C.Z) / (A.X - C.X);
            mu3 = (B.Y - D.Y) / (A.X - C.X) - (G.Y - J.Y) / (F.X - H.X);
            m_Transform.M23 = (float)(((mu3 / mu1) * pi1 - pi3) / (pi2 - pi1 * (mu2 / mu1)));
            m_Transform.M22 = (float)(-1 * ((mu2 / mu1) * m_Transform.M23 + (mu3 / mu1)));
            m_Transform.M21 = ((B.Y - D.Y) / (A.X - C.X) - (m_Transform.M22) * (A.Y - C.Y) / (A.X - C.X)) - (m_Transform.M23 * (A.Z - C.Z) / (A.X - C.X));
            m_Transform.M24 = B.Y - (A.X * m_Transform.M21) - (A.Y * m_Transform.M22) - (A.Z * m_Transform.M23);

            // compute third row of T matrix
            pi1 = ((V.Y - Z.Y) - ((A.Y - C.Y) / (A.X - C.X)) * (V.X - Z.X));
            pi2 = ((V.Z - Z.Z) - ((A.Z - C.Z) / (A.X - C.X)) * (V.X - Z.X));
            pi3 = (((B.Z - D.Z) / (A.X - C.X)) * (V.X - Z.X) - (L.Z - K.Z));
            mu1 = (F.Y - H.Y) / (F.X - H.X) - (A.Y - C.Y) / (A.X - C.X);
            mu2 = (F.Z - H.Z) / (F.X - H.X) - (A.Z - C.Z) / (A.X - C.X);
            mu3 = (B.Z - D.Z) / (A.X - C.X) - (G.Z - J.Z) / (F.X - H.X);
            m_Transform.M33 = (float)(((mu3 / mu1) * pi1 - pi3) / (pi2 - pi1 * (mu2 / mu1)));
            m_Transform.M32 = (float)(-1 * ((mu2 / mu1) * m_Transform.M33 + (mu3 / mu1)));
            m_Transform.M31 = ((B.Z - D.Z) / (A.X - C.X) - (m_Transform.M32) * (A.Y - C.Y) / (A.X - C.X)) - (m_Transform.M33 * (A.Z - C.Z) / (A.X - C.X));
            m_Transform.M34 = B.Z - (A.X * m_Transform.M31) - (A.Y * m_Transform.M32) - (A.Z * m_Transform.M33);
        }
    }
}
